#pragma once

#include "HapticAPI.h"
#include "FcsHapticMASTER.h"
#include "FcsConstantForce.h"
#include "FcsSphere.h"
#include "FcsBlock.h"
#include "ScopeAPI.h"
#include <vector>
#include <string>
#include <algorithm>
#pragma comment(lib, "HapticAPI.lib")
#pragma comment(lib, "ScopeAPI.lib")

/// <summary>
/// matlab-y stuff
/// </summary>
#pragma comment(lib, "libdfblas.lib")
#pragma comment(lib, "libdflapack.lib")
#pragma comment(lib, "libemlrt.lib")
#pragma comment(lib, "libeng.lib")
#pragma comment(lib, "libfixedpoint.lib")
#pragma comment(lib, "libmat.lib")
#pragma comment(lib, "libmex.lib")
#pragma comment(lib, "libmwblas.lib")
#pragma comment(lib, "libmwlapack.lib")
#pragma comment(lib, "libmwmathutil.lib")
#pragma comment(lib, "libmwservices.lib")
//#pragma comment(lib, "libmwsl_solver_rtw.lib")
#pragma comment(lib, "libmx.lib")
#pragma comment(lib, "libut.lib")

using namespace std;

namespace matlab {
#include "mat.h" // for matlab-y stuff
#include "matrix.h"
}

enum HM_CONSTANTS {
	CLCK_BIT = 1,		// clock bit within FCSPRM_PARALLELPORT_CONTROL
	TRIG_BIT = 2,		// trigger bit within FCSPRM_PARALLELPORT_CONTROL
	NAXES = 3,			// primary axes
	NCOORDS = 4,		// primary coordinates
	NSENSORS = 15,		// total number of sensors
	NSENSORST = 16,		// sensors including time
	NCALIBPTS = 1000,	// number of calibration points
	MAXFREQ	= 1000,		// maximum collection frequency
	TRIG_LO = 0,		// false = trigger low 
	TRIG_HI = 1,		// true	 = trigger high
	CLCK_LO = 1,		// true	 = clock low
	CLCK_HI = 0,		// false = clock high
	MAX_FORCE = 100		// maximum force to be exterted by the haptic master
};
enum AXIS_LIST {
	X = 0,
	Y = 1,
	Z = 2,
	THETA = 3
};
enum SENSOR_LIST {
	T = 0,			// time
	PX, PY, PZ,		// position
	VX, VY, VZ,		// velocity
	AX, AY, AZ,		// acceleration
	FX, FY, FZ,		// force
	GX, GY, GZ,		// gimbal angles
	PXC, PYC, PZC,	// commanded position
	VXC, VYC, VZC,	// commanded velocity
	FXC, FYC, FZC	// commanded force
};

struct CPerturbation {
	vector<vector<double>>	position;	// NSAMPLES x 3 
	vector<vector<double>>	velocity;	// NSAMPLES x 3
	//vector<vector>double>>	force;		// NSAMPLES x 3
	vector<vector<double>>	stiffness;	// NSAMPLES x 9
	vector<vector<double>>	damping;	// NSAMPLES x 9
	double					freq;		// sampling rate (Hz)
	double					timePeriod; //  = 1/freq (s)
	unsigned				nSamples;	// length of the perturbation in samples
	double					maxT;		// length of the perturbation in time (s)
	double					delay;		// delay in onset of perturbation
};

class ChmDevice
{
private:
	/// <summary>
	/// hardware settings
	/// </summary>
	wstring					deviceName;		// typically, "vmd"
	wstring					ipAddress;		// ip address of the device
	unsigned				port;			// port number of the device
	CFcsHapticMASTER*		hm;				// pointer to the haptic master object

	/// <summary>
	/// haptic environment settings
	/// </summary>
	FCSSTATE				currentState;	// current state of the haptic master
	double					mass;			// mass to simulate
	double					dampingRatio;	// damping ratio
	double					envK[3][3];		// environment stiffness
	double					envB[3][3];		// environment damping
	vector<double>			biasForce;		// bias force

	double					force2Render[3];// force to render
	double					dPos[3];		// error in position
	double					dVel[3];		// error in velocity

	vector<double>			tarPos;			// target position (m)
	vector<double>			tarGim;			// target gimbal
	vector<double>			tarFor;			// target force (N)
	double					tarFTol;		// tolerance in target force (N)
	double					tarGimbalTol;	// tolerance in the target gimbal (degrees)

	volatile bool			tarHeldFlag;	// flag to determine if target has been held
	double					tarHoldT;		// duration for which target must be held
	double					tarHeldT;		// duration for which target is held

	/// <summary>
	/// objects for workspace limits
	/// </summary>
	CFcsBlock*				pBlock;			// haptic block (for setting up cuboidal limits)
	CFcsSphere*				pSphere;		// haptic sphere (for setting up spherical limits)
	
	/// <summary>
	/// data collection settings
	/// </summary>
	volatile bool			armedFlag;		// flag to start pretrigger data collection
	volatile bool			collectFlag;	// flag to start data collection
	volatile double			duration;		// duration for data collection
	volatile double			pretrigger;		// pretrigger duration
	vector<double>			dataVec;		// data collected for one time instant
	vector<vector<double>>	dataMat;		// data collected for a trial
	vector<vector<double>>	dataPreTrig;	// pretrigger data holder, will be used as a circular buffer
	volatile unsigned		cbCount;		// count for the circular buffer (pretrigger data)

	FcsInfoBuffer*			dataBuf;		// buffer used for collecting data -- internal operations only
	wstring					fileName;		// filename for data collection
	matlab::MATFile			*pmat;			// matlab file pointer

	LARGE_INTEGER			tickFreq;		// tick frequency
    LARGE_INTEGER			tCurrent;		// current time -- in ticks
	double					curT;			// current time -- in seconds
	double					prevT;			// previous time -- in seconds
	volatile double			startT;			// start time
	vector<double>			curPos;			// current position
	vector<double>			curVel;			// current velocity
	vector<double>			curAcc;			// current acceleration
	vector<double>			curFor;			// current force
	vector<double>			curGim;			// current gimbal

	/// <summary>
	/// device calibration settings
	/// </summary>
	vector<double> 			zeroPos;		// position_{zero}
	vector<double>			zeroGim;		// gimbal_{zero}

	/// <summary>
	/// perturbation settings
	/// </summary>
	//vector<CPerturbation>	pertInfo;		// perturbation data
	CPerturbation			*pertInfo;
	volatile bool			pertEnableFlag;	// perturbation is enabled
	bool					pertOnFlag;		// perturbation is on
	double					pertT;			// perturbation onset time
	vector<double>			pertZero;		// zero position to use during perturbation
	vector<double>			pertPos;
	vector<double>			pertVel;
	vector<vector<double>>	pertK;
	vector<vector<double>>	pertB;
public:
	ChmDevice(wstring);
	~ChmDevice(void);

	/// <summary>
	/// hardware functions
	/// </summary>
	bool Connect(void);										// connect to the hardware (i.e., initialize)
	bool Disconnect(void);									// disconnect from the hardware

	/// <summary>
	/// haptic environment functions
	/// </summary>
	bool SetState(FCSSTATE state);							// set current the hapticMASTER state
	FCSSTATE GetState(void);								// get the current hapticMASTER state
	bool SetMass(double mass);								// set the mass
	void SetEnvironment(double k[3][3], double b[3][3]);	// set the haptic environment 
	void SetEnvironment(double k[3][3], double zeta);		// set the haptic environment

	void SetBiasForce(vector<double>);						// set the bias force
	void SetTargetForce(vector<double>, double, double);	// set the target force
	void SetTargetPosition(vector<double>, double, double);	// set the target position
	void SetTargetGimbal(vector<double>, double);			// set the target gimbal

	inline void RenderHaptics(void);						// render the haptic force

	/// <summary>
	/// workspace limits
	/// </summary>
	void CreateSphericalLimits(double radius, double center[3]);	// set spherical limits
	void CreateCuboidalLimits(double lengths[3], double center[3]);	// set cuboidal limits
	
	/// <summary>
	/// device calibration functions
	/// </summary>
	bool ZeroForce(void);					// zero the forces
	vector<double> ZeroPosition(void);		// zero the position
	vector<double> ZeroGimbal(void);		// zero the gimbal

	/// <summary>
	/// data collection functions
	/// </summary>
	vector<double> GetPosition(void);		// current hm position
	vector<double> GetVelocity(void);		// current hm velocity
	vector<double> GetForce(void);			// current hm force
	vector<double> GetGimbal(void);			// current hm gimbal

	vector<double> FetchPosition(void);		// current position, as stored in the class
	vector<double> FetchForce(void);		// current force, as stored in the class
	vector<double> FetchGimbal(void);		// current position, as stored in the class
	vector<double> FetchZeroPosition(void);	// zero position, as stored in the class
	vector<double> FetchZeroGimbal(void);	// zero gimbal, as stored in the class

	bool ArmCollection(wstring, double, double);	// arm the data collection
	bool EnableDataCollection(void);				// enable data collection by the RenderHaptics loop
	bool EnableDataCollection(bool);				// enable data collection by the RenderHaptics loop -- with input for enabling perturbation
	wstring SaveData(void);							// save the file

	/// <summary>
	/// perturbation related functions
	/// </summary>
	bool LoadPerturbation(wstring);				// load the perturbation file

	/// <summary>
	/// error handling -- eventually needs to be replaced by an exception handling class
	/// </summary>
	wstring errStr;

	// File for debug purposes

	FILE *fp;	
};


// this function is inline to make it run faster
inline void ChmDevice::RenderHaptics()
{
	unsigned	axis			= 0;
	unsigned	axis2			= 0;
	unsigned	idx				= 0;
	unsigned	nextIdx			= 0;
	static unsigned	count		= 0;
	double		sum				= 0.0;
	double		deltaT			= 0.0;

	fp=fopen("E://Shirpaa_Test_data//Debug_Files//timeRenderHaptics.txt", "a");

	// get the current time
	QueryPerformanceCounter( &tCurrent );	
	curT = (double) tCurrent.QuadPart/tickFreq.QuadPart; 
			
	// calculate the force to be rendered
	if (pertOnFlag) { // if perturbation is on
		// get index of perturbation and corresponding time increment
		deltaT = curT - pertT;
		idx = floor(deltaT/pertInfo->timePeriod);
		nextIdx = idx + 1;
		
		if ( nextIdx < pertInfo->nSamples ) {
			sum = (deltaT - (idx*pertInfo->timePeriod)) / pertInfo->timePeriod; // (increment in time since last idx) / (sample period)
			for (axis = X; axis<NAXES; axis++) {
				// linearly interpolate to get the appropriate pertPos and pertVel for the current time step

				pertPos[axis] = (pertInfo->position[nextIdx][axis] - pertInfo->position[idx][axis]) * sum + pertInfo->position[idx][axis];
				pertVel[axis] = (pertInfo->velocity[nextIdx][axis] - pertInfo->velocity[idx][axis]) * sum + pertInfo->velocity[idx][axis];
				// linearly interpolate to get the stiffness and damping for the current time step
				for (axis2 = X; axis2<NAXES; axis2++) {
					//pertK[axis][axis2] = (pertInfo->stiffness[nextIdx][axis*NAXES + axis2] - pertInfo->stiffness[idx][axis*NAXES + axis2]) * sum + pertInfo->stiffness[idx][axis*NAXES + axis2];
					//pertB[axis][axis2] = (pertInfo->damping[nextIdx][axis*NAXES + axis2] - pertInfo->damping[idx][axis*NAXES + axis2]) * sum + pertInfo->damping[idx][axis*NAXES + axis2];
					pertK[axis][axis2] = 0.0;
					pertB[axis][axis2] = 0.0;
					if (axis == axis2) {
						pertK[axis][axis2] = 50000.0;
						pertB[axis][axis2] = 632.5;
					}
				}
				// error in the position and velocity (controlled signals)
				//dPos[axis] = pertPos[axis] - curPos[axis] + pertZero[axis]; //error in position 
				//dVel[axis] = pertVel[axis] - curVel[axis]; // error in velocity
				dPos[axis] = .01 + zeroPos[axis] - curPos[axis]; // error in position
				dVel[axis] = 0.1 - curVel[axis]; // error in velocity (assumes set point = 0)
			}

			// calculate the appropriate force2Render
			for (axis = X; axis<NAXES; axis++) {
				force2Render[axis] =   pertK[axis][X] * dPos[X] + pertB[axis][X] * dVel[X]
									 + pertK[axis][Y] * dPos[Y] + pertB[axis][Y] * dVel[Y]
									 + pertK[axis][Z] * dPos[Z] + pertB[axis][Z] * dVel[Z]
									 + biasForce[axis]; // + pertForce[axis] -- include a force perturbation eventually
			}

		} // end if (still applying perturbation waveform)
		else { // done applying perturbation
			pertOnFlag = false;
			pertEnableFlag = false;
		}
	} // end if (pertOnFlag)

	// NOTE: this is an IF statement on purpose. Do *NOT* change this to an ELSE.
	// Checking if pertOnFlag for a second time is done on purpose, since the 
	// previous if can set pertOnFlag to false
	if (!pertOnFlag) { // not applying a perturbation

		// error in the position and velocity (controlled signals)
		for (axis = X; axis<NAXES; axis++) {
			dPos[axis] = zeroPos[axis] - curPos[axis]; // error in position
			dVel[axis] = -curVel[axis]; // error in velocity (assumes set point = 0)
		}

		// calculate the appropriate force2Render
		for (axis = X; axis<NAXES; axis++) {
			force2Render[axis] =   envK[axis][X] * dPos[X] + envB[axis][X] * dVel[X]
								 + envK[axis][Y] * dPos[Y] + envB[axis][Y] * dVel[Y]
								 + envK[axis][Z] * dPos[Z] + envB[axis][Z] * dVel[Z]
								 + biasForce[axis];
		}
	}

	// check for force limits
	for (axis=X; axis<NAXES; axis++) {
		// limit the maximum force to be rendered
		if (force2Render[axis] > MAX_FORCE) 
			force2Render[axis] = MAX_FORCE;
		else if (force2Render[axis] < -MAX_FORCE)
			force2Render[axis] = -MAX_FORCE;
		// check for workspace limits?
	}

#if _DEBUG
	for (axis=X; axis<NAXES; axis++) {
		curPos[axis] = 0.0;
		curVel[axis] = 0.0;
		curAcc[axis] = 0.0;
		curFor[axis] = 0.0;
		curGim[axis] = 0.0;
	}
#else
	// render the force and get the current values
	hm->SetForceGetInfo(force2Render, dataBuf);

	// store the current values
	for (idx = 0; idx < (dataBuf->NrInfoBlocksLastSent); idx++) {
		switch (dataBuf->InfoBlock[idx].Param) {
		case FCSPRM_POSITION:
			for (axis = X; axis<NAXES; axis++)
				curPos[axis] = dataBuf->InfoBlock[idx].Info[axis];
			break;
		case FCSPRM_VELOCITY:
			for (axis = X; axis<NAXES; axis++)
				curVel[axis] = dataBuf->InfoBlock[idx].Info[axis];
			break;
		case FCSPRM_ACCELERATION:
			for (axis = X; axis<NAXES; axis++)
				curAcc[axis] = dataBuf->InfoBlock[idx].Info[axis];
			break;
		case FCSPRM_FORCE:
			for (axis = X; axis<NAXES; axis++)
				curFor[axis] = dataBuf->InfoBlock[idx].Info[axis];
			break;
		case FCSPRM_POTMETERS:
			for (axis = X; axis<NAXES; axis++)
				curGim[axis] = dataBuf->InfoBlock[idx].Info[axis];
			break;

		default:
			break;
		}
	}
#endif

	/// <summary>
	/// If attempting to collect data, i.e., collectFlag = true
	///		If target has not been held for the desired time, i.e., targetHeldFlag = false
	///			Reinitialize start time
	///			If target is matched now
	///				Increment target held time
	///				If target was held for the desired time 
	///					Send Trigger out
	///					Declare target held, i.e, targetHeldFlag = true
	///			Else
	///				Reset target held time
	///		Check ***AGAIN*** if target has been held for the desired time
	///			If continuing to record data, i.e., time elapsed since trigger is < duration of data collection
	///				Record data, i.e., push davatVec into dataMat
	///			Else
	///				Reset trigger
	///				Disable collection, i.e., collectFlag = false
	///				Reset time for which target was held, i.e., tarHeldFlag = false; tarHeldT = 0.0;
	///				Save data, including the startTime
	///	If not collecting data, or if not triggered yet,
	///		Store data in the circular buffer
	/// </summary>
	deltaT = curT - prevT;
	double interval = (double)((double)1/(double)MAXFREQ);
	//if (deltaT > 0.00035)  { // limit the max collection rate to ~2500Hz 
	if (deltaT > interval)  {

		prevT = curT; // reinitialize previous time point for data collection

		// record current data, temporarily -- used later during data collection or in a circular buffer
		dataVec[T]	= curT;
		for (axis = X; axis<NAXES; axis++) {
			dataVec[PX + axis] = curPos[axis];
			dataVec[VX + axis] = curVel[axis];
			dataVec[AX + axis] = curAcc[axis];
			dataVec[FX + axis] = curFor[axis];
			dataVec[GX + axis] = curGim[axis];

			// command variables
			//dataVec[PXC + axis] = dPos[axis];
			//dataVec[VXC + axis] = dVel[axis];
			//dataVec[FXC + axis] = force2Render[axis];
		}

		

		// if collecting data
		if (collectFlag) {
			
			// if target has not been held for the desired duration
			if (!tarHeldFlag) {
				
				startT = pertT = curT; // reset start time and perturbation onset time
				
				// get current resultant error in force
				for (axis = 0; axis<3; axis++)
					sum += pow(curFor[axis] - tarFor[axis], 2);
				
				// if the target is matched
				if (sqrt(sum) < tarFTol) {
					tarHeldT += deltaT;
					if ( tarHeldFlag = (tarHeldT >= tarHoldT) ) { // set the tarHeldFlag and check if it is true 
						#if !_DEBUG
							hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, TRIG_BIT, TRIG_HI);
							
							// start scope
							if (ScopeStart() != SCOPE_NO_ERROR)
							{
								// error: Scope did NOT start
							}
						#endif
						armedFlag = false;
					}
				}
				// if the target is not matched
				else {
					tarHeldT = 0.0;
				}
			}
			// NOTE: Please do *NOT* replace the following with an ELSE...
			///		 it is intentionally left to be another IF statement.
			///		 This avoids an extra time step when it comes to collecting
			///		 data. Please trace the code or read the summary above.

			// if target has been held for the desired duration
			if (tarHeldFlag) {
				// if continuing to record data
				double total_time = curT - startT;
				if (total_time < duration) {
					dataMat.push_back(dataVec);

					// if perturbation delay has been reached but has not been started already, set to apply the perturbation
					if ( ((curT-startT) >= pertInfo->delay) && (!pertOnFlag) && pertEnableFlag ) {
						pertOnFlag = true; // enable perturbation
						pertT = curT; // set the perturbation onset time
						pertZero = vector<double>(curPos);
					}
						
				}
				else {
#if !_DEBUG
					// set trigger pulse low
					hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, TRIG_BIT, TRIG_LO);
			
					if ( ScopeStop() != SCOPE_NO_ERROR )	// try stopping the scope
					{
						// error stopping scope
					}
#endif
					collectFlag = false;
					tarHeldFlag = false;
					armedFlag = false;
					
					// NOTE: Eventually, this needs to be run in a separate thread, 
					// so as not to interrupt the haptic rendering
					//SetState(FCSSTATE_FIXED);
					SaveData();
					//SetState(FCSSTATE_NORMAL);
				}
			}
		} // end if collectFlag
		
		if (!tarHeldFlag && armedFlag) {
			// Store dataVec in the circular buffer
			dataPreTrig[cbCount] = vector<double>(dataVec);
			cbCount++;
			if (cbCount == dataPreTrig.size())
				cbCount = 0;
		} 
		
	} // end if deltaT > 0.00035

	QueryPerformanceCounter( &tCurrent );	
	double curTEnd = (double) tCurrent.QuadPart/tickFreq.QuadPart; 
	fprintf(fp, "%f\n", (curTEnd- curT));

	fclose (fp);
	
}